# ----------------------------
# coding: utf-8
# 开发作者 : 钱嘉龙
# 创建日期 : 2022/2/22 15:18
# ----------------------------


import cv2 as cv
import imutils
import numpy as np
import serial
import math
from PIL import Image, ImageDraw, ImageFont
import time
import binascii


show_area = 0
area_max = 5500
area_min = 2000
color_dict = {'red': 0.0, 'green': 0.0, 'blue': 0.0}


class ColorDetection(object):
    @staticmethod
    # 获取本地图片
    def get_images(name):
        image = cv.imread('F:\\EngineeringDocuments\\Logistics_Handling_Robot\\images\\{0}.jpg'.format(name))
        return image

    @staticmethod
    # 获取摄像头
    def camera_get_images(cap):
        ret, image = cap.read()
        return ret, image

    @staticmethod
    # 显示指定图片
    def show_image(name, src, height, save):
        cv.namedWindow(name, cv.WINDOW_AUTOSIZE)
        if height == 0:
            pass
        else:
            src = imutils.resize(src, height=height)
        cv.imshow(name, src)
        if save:
            cv.imwrite('F:\\EngineeringDocuments\\Logistics_Handling_Robot\\images\\{0}.jpg'.format(name), src)

    @staticmethod
    # 更改图片的对比度、亮度函数     --->     c:对比度;     b:亮度;
    def contrast_brightness(image, c, b):
        height, width, channels = image.shape[0:3]
        black = np.zeros([height, width, channels], image.dtype)
        dst = cv.addWeighted(image, c, black, 1 - c, b)
        return dst

    @staticmethod
    # 二值化
    def threshold(gary):
        # ret, binary = cv.threshold(gary, 0, 255, cv.THRESH_BINARY)
        binary = cv.adaptiveThreshold(gary, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY, 15, 5)
        return binary

    @staticmethod
    # 高斯模糊
    def gaussian_blur(image, size):
        return cv.GaussianBlur(image, (size, size), 0)

    @staticmethod
    # 转换灰度图片
    def convert_gray_scale(image):
        return cv.cvtColor(image, cv.COLOR_BGR2GRAY)

    @staticmethod
    # 获取卷积核
    def get_kernel(model, size):
        if model == 0:
            kernel = cv.getStructuringElement(cv.MORPH_RECT, (size, size))
        else:
            kernel = cv.getStructuringElement(cv.MORPH_ELLIPSE, (size, size))
        return kernel

    @staticmethod
    # 形态学操作
    def morphology_process(binary, model, kernel, times):
        if model == 0:
            open_image = cv.morphologyEx(binary, cv.MORPH_OPEN, kernel, iterations=times)
        else:
            open_image = cv.morphologyEx(binary, cv.MORPH_CLOSE, kernel, iterations=times)
        return open_image

    @staticmethod
    # 边缘检测
    def detect_edges(image, dst, low_threshold=45, high_threshold=125):
        dst_copy = dst.copy()
        canny = cv.Canny(image, low_threshold, high_threshold)
        contours = cv.findContours(canny, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)[0]
        for i, contour in enumerate(contours):
            area = cv.contourArea(contour)
            print(area)
            cv.drawContours(dst_copy, contours, i, (255, 255, 0), 2)
        return dst_copy, contours

    @staticmethod
    # 角点检测
    def harris_detect(gray, image):
        image_copy = image.copy()
        harris = cv.cornerHarris(gray, 10, 5, 0.04)
        image_copy[harris > 0.1 * harris.max()] = [255, 255, 255]
        return image_copy, harris

    @staticmethod
    # 获取外接矩形
    def bounding_boxes(contours, image, number):
        color = {0: 'red', 1: 'brown', 2: 'orange', 3: 'yellow', 4: 'green', 5: 'blue', 6: 'purple'}
        image_copy = image.copy()
        for i, contours in enumerate(contours):
            area = cv.contourArea(contours)
            if not(area_min < area < area_max):
                continue
            x, y, w, h = cv.boundingRect(contours)
            print(color[number])
            cv.putText(image_copy, '{0}'.format(color[number]), (x, y-10),
                       cv.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 255), 2)
            mm = cv.moments(contours)
            if not(mm['m00'] == 0):
                cx = mm['m10']/mm['m00']
                cy = mm['m01']/mm['m00']
                cv.circle(image_copy, (int(cx), int(cy)), 5, (255, 255, 255), -1)
            cv.rectangle(image_copy, (x, y), (x+w, y+h), (255, 255, 255), 2)
        return image_copy

    @staticmethod
    # 获取最小外接矩形
    def min_bounding_boxes(contours, image, number):
        """
        :param contours: 传入的所有轮廓
        :param image: 所需要输出的图片
        :param number: 对应的何种颜色
        :return:
            image_copy                  传回的图片
            (center_x, center_y)        重心点的xy坐标
            return_color                传回颜色
            return_box                  传回最小外接矩形的四个坐标点
            return_angle                传回粗略角度
            (core_x, core_y)            中心点的xy坐标
        """
        color = {0: 'red', 1: 'green', 2: 'blue'}
        image_copy = image.copy()
        # 返回重心x, y坐标
        center_x, center_y = [], []
        # 返回颜色
        return_color = []
        # 返回最小外接矩形的四个点位
        return_box = []
        # 返回粗略角度
        return_angle = []
        # 返回中心x, y坐标
        core_x, core_y = [], []
        for i, contours in enumerate(contours):
            area = cv.contourArea(contours)
            if not (area_min < area < area_max):
                continue
            if show_area:
                print("area:", area)
            rect = cv.minAreaRect(contours)
            # 中心点
            (x, y) = rect[0]
            core_x.append(x)
            core_y.append(y)
            # 宽高
            # (w, h) = rect[1]
            # 角度按顺时针方向每个象限从0-90
            angle = int(rect[2])
            # 最小外接矩形的四个点位，四个点位的排序
            # 在0-90°中，按照左 下，左上，右上，右下排序
            box = cv.boxPoints(rect)
            box = np.int0(box)
            # 显示角度
            # cv.putText(image_copy, '{0}'.format(angle), box[1],
            #            cv.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2)
            mm = cv.moments(contours)
            if not (mm['m00'] == 0):
                cx = int(mm['m10'] / mm['m00'])
                cy = int(mm['m01'] / mm['m00'])
                center_x.append(cx)
                center_y.append(cy)
                if len(return_color) == 0:
                    return_color.append(color[number])
                return_box.append(box)
                # return_angle.append(angle)
                # cv.circle(image_copy, (cx, cy), 4, (255, 255, 255), -1)
                # cv.circle(image_copy, (int(x), int(y)), 4, (0, 255, 0), -1)
                # 显示颜色
                cv.putText(image_copy, '{0}'.format(color[number]), (cx-20, cy-10),
                           cv.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 255), 2)
            # image_copy = cv.drawContours(image_copy, [box], -1, (255, 255, 255), 2)
        return image_copy, (center_x, center_y), return_color, return_box, return_angle, (core_x, core_y)

    @staticmethod
    # 计算两点之间的距离
    def calculate_long(pts1, pts2):
        distance = math.pow((pts1[0] - pts2[0]), 2) + math.pow((pts1[1] - pts2[1]), 2)
        distance = math.sqrt(distance)
        return distance

    @staticmethod
    # 判断物块位置
    def decide_position(color, core_x):
        color_pos = ['', '', '']
        if color == ['red']:
            color_dict['red'] = core_x
        elif color == ['green']:
            color_dict['green'] = core_x
        elif color == ['blue']:
            color_dict['blue'] = core_x

        if color_dict['red'] != 0.0 and color_dict['green'] != 0.0 and color_dict['blue'] != 0.0:
            max_x = max(color_dict['red'], color_dict['green'], color_dict['blue'])
            min_x = min(color_dict['red'], color_dict['green'], color_dict['blue'])

            if min_x == color_dict['red']:
                color_pos[0] = '红'
            elif min_x == color_dict['green']:
                color_pos[0] = '绿'
            elif min_x == color_dict['blue']:
                color_pos[0] = '蓝'

            if max_x == color_dict['red']:
                color_pos[2] = '红'
            elif max_x == color_dict['green']:
                color_pos[2] = '绿'
            elif max_x == color_dict['blue']:
                color_pos[2] = '蓝'

            if '红' not in color_pos:
                color_pos[1] = '红'
            elif '绿' not in color_pos:
                color_pos[1] = '绿'
            elif '蓝' not in color_pos:
                color_pos[1] = '蓝'
        # print('color_position', color_pos)

        return color_pos


    @staticmethod
    # 二维码检测识别
    def detect_QR_code(gray):
        codeinfo = ''
        points = [[[0, 0]]]
        # 实例化
        qrcoder = cv.QRCodeDetector()
        # qr检测并解码
        codeinfo, points, straight_qrcode = qrcoder.detectAndDecode(gray)
        # print(points)
        # 绘制qr的检测结果
        if points is None:
            return 0
        else:
            cv.drawContours(gray, [np.int32(points)], 0, (0, 0, 255), 2)
            return codeinfo, points


    @staticmethod
    def QRcode_draw(dst_image, QRcode_box, barcodeData):
        if QRcode_box is None:
            return 0
        cv.rectangle(dst_image, (int(QRcode_box[0][0][0]), int(QRcode_box[0][0][1])),
                     (int(QRcode_box[0][2][0]), int(QRcode_box[0][2][1])), (0, 0, 255), 2)
        cv2img = cv.cvtColor(dst_image, cv.COLOR_BGR2RGB)  # cv2和PIL中颜色的hex码的储存顺序不同
        pilimg = Image.fromarray(cv2img)
        barcodeData_str = ''.join(barcodeData)
        # PIL图片上打印汉字
        draw = ImageDraw.Draw(pilimg)  # 图片上打印
        font = ImageFont.truetype("simhei.ttf", 20, encoding="utf-8")  # 参数1：字体文件路径，参数2：字体大小
        draw.text((int(QRcode_box[0][0][0]), int(QRcode_box[0][0][1])-22), barcodeData_str, (255, 0, 0), font=font)  # 参数1：打印坐标，参数2：文本，参数3：字体颜色，参数4：字体
        # PIL图片转cv2 图片
        cv2charimg = cv.cvtColor(np.array(pilimg), cv.COLOR_RGB2BGR)
        return cv2charimg


    @staticmethod
    # 判断任务抓取顺序和位置 0 1 2 分别从 左--->右
    def mission_process(mission, color_position):
        grab_pos = ''
        # print('mission', mission, 'color_position', color_position)
        if mission[0] == '红':
            red = [i for i, x in enumerate(color_position) if x == '红']
            # print(red, type(red), len(red))
            # print(str(red[0]), type(str(red[0])), len(str(red[0])))
            grab_pos += str(red[0])
        elif mission[0] == '绿':
            green = [i for i, x in enumerate(color_position) if x == '绿']
            grab_pos += str(green[0])
        elif mission[0] == '蓝':
            blue = [i for i, x in enumerate(color_position) if x == '蓝']
            grab_pos += str(blue[0])

        if mission[2] == '红':
            red = [i for i, x in enumerate(color_position) if x == '红']
            grab_pos += str(red[0])
        elif mission[2] == '绿':
            green = [i for i, x in enumerate(color_position) if x == '绿']
            grab_pos += str(green[0])
        elif mission[2] == '蓝':
            blue = [i for i, x in enumerate(color_position) if x == '蓝']
            grab_pos += str(blue[0])
        return grab_pos


    @staticmethod
    def usart_wait_open():
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        mark = 'A'
        while True:
            # ser.write(bytes('A', encoding='utf-8'))
            ans = ser.read(1)
            if ans == mark.encode():
                break
        print("usart_wait_open and camera open success!")
        return 1


    @staticmethod
    def usart_send_QRcode(mission):
        mission_send = ''
        if mission[0] == '红':
            mission_send += 'r'
        elif mission[0] == '绿':
            mission_send += 'g'
        elif mission[0] == '蓝':
            mission_send += 'b'

        if mission[2] == '红':
            mission_send += 'r'
        elif mission[2] == '绿':
            mission_send += 'g'
        elif mission[2] == '蓝':
            mission_send += 'b'
        qrcode = mission_send
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        # mark = 'B'
        while True:
            # ans = ser.read(1)
            # if ans == mark.encode():
            #     break
            for i in range(0, 300):
                ser.write(bytes('M', encoding='utf-8'))
                ser.write(bytes(qrcode, encoding='utf-8'))
                # print("wait MCU get QRcode info......")
            break
        print("QRcode info send success!")
        return 1


    @staticmethod
    def usart_wait_detect_color_1():
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        mark = 'B'
        while True:
            ans = ser.read(1)
            if ans == mark.encode():
                break
        print("usart_wait_detect_color_1 and camera open success!")
        return 1

    @staticmethod
    def usart_wait_detect_color_2():
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        mark = 'C'
        while True:
            ans = ser.read(1)
            if ans == mark.encode():
                break
        print("usart_wait_detect_color_2 and camera open success!")
        return 1

    @staticmethod
    def usart_wait_detect_location_1():
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        mark = 'D'
        while True:
            ans = ser.read(1)
            if ans == mark.encode():
                break
        return 1

    @staticmethod
    def usart_wait_detect_location_2():
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        mark = 'E'
        while True:
            ans = ser.read(1)
            if ans == mark.encode():
                break
        return 1

    @staticmethod
    def usart_send_color_1(grab_pos):
        qrcode = grab_pos
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        # mark = 'B'
        while True:
            # ans = ser.read(1)
            # if ans == mark.encode():
            #     break
            for i in range(0, 300):
                ser.write(bytes('P', encoding='utf-8'))
                ser.write(bytes(qrcode, encoding='utf-8'))
                # print("wait MCU get color position......")
            break
        print("send msg:", qrcode)
        print("grab position send success!")
        return 1


    @staticmethod
    def usart_send_color_2(grab_pos):
        qrcode = grab_pos
        ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.01)
        # mark = 'B'
        while True:
            # ans = ser.read(1)
            # if ans == mark.encode():
            #     break
            for i in range(0, 300):
                ser.write(bytes('Q', encoding='utf-8'))
                ser.write(bytes(qrcode, encoding='utf-8'))
                # print("wait MCU get color position......")
            break
        print("send msg:", qrcode)
        print("grab position send success!")
        return 1


    @staticmethod
    def get_laser_distance():
        # s = serial.Serial("COM3", 9600)  # 初始化串口
        s = serial.Serial("/dev/ttyUSB0", 9600)  # 初始化串口
        while True:
            n = s.inWaiting()  # 串口接收
            if n:
                data = str(binascii.b2a_hex(s.read(n)))[2:-1]    # Hex转换成字符串
                # print(data, type(data), len(data))             # 字符串输出
                distance_str = data[10:14]
                time.sleep(0.1)
                if distance_str != '':
                    distance_oct = int(distance_str, 16)
                    # laser_distance = distance_oct
                    if 5 <= distance_oct <= 3999:
                        # print(distance_oct, type(distance_oct))
                        return distance_oct
